#ifndef __SABER_CONTROL_H__
#define __SABER_CONTROL_H__

#include <array>
#include "Cybergear_control.hpp"
#include <cmath>

//                   Saber DH参数表
//   i     theta(弧度) d(mm)    a(mm)    alpha(弧度)
//   1     theta1     195       0        pi/2
//   2     theta2      0      111.59       0
//   3     theta3      0      111.59       0
//   4     theta4     8.34      0        pi/2
//   5     theta5   118.36      0       -pi/2
//   6     theta6   218.71      0          0

//要用到的DH参数
#define d1 195
#define a2 111.59
#define a3 111.59
#define d4 8.34
#define d5 118.36
#define d6 218.71

class Saber{
public:
    uint8_t solution_num;
    std::array<std::array<double,6>,8> solution_list;
    std::array<double,6> theta;

    Saber(void);
    ~Saber(){};
    uint8_t inverse_kinematics_solver(const double* target_pose);
    double angle_adjust_PI(double angle);
    double angle_adjust_2PI(double angle);
    uint8_t solution_choose(void);
};




#endif